/*
  WebSocket client on ESP32 to talk to 
  ROS_Bridge_Suite on Ubuntu PC. 
  Communication Working. Ros_Brige_Suite Pkg receive message
  To DO in Future :Not add ROS message pub/receiver yet
  use WebSockets lib
 Apr-10, 2024, Athlon
*/

#include <motor.h>
#include <main.h>
// #include <blink_ble.h>
#include <Arduino.h>
#include <Wire.h>
#include <SimpleFOC.h>
#include <WiFi.h>
#include "arduino_secrets.h"
#include <WebSocketsClient.h>
#include <ArduinoJson.h>

///////please enter your sensitive data in the Secret tab/arduino_secrets.h
/////// WiFi Settings ///////
char ssid[] = SECRET_SSID;
char pass[] = SECRET_PASS;

char serverAddress[] = "192.168.1.10";  // server address
int port = 8080;

WiFiClient wifi;

// Websocket function
WebSocketsClient webSocket;
#define USE_SERIAL Serial

void hexdump(const void *mem, uint32_t len, uint8_t cols = 16) {
	const uint8_t* src = (const uint8_t*) mem;
	USE_SERIAL.printf("\n[HEXDUMP] Address: 0x%08X len: 0x%X (%d)", (ptrdiff_t)src, len, len);
	for(uint32_t i = 0; i < len; i++) {
		if(i % cols == 0) {
			USE_SERIAL.printf("\n[0x%08X] 0x%08X: ", (ptrdiff_t)src, i);
		}
		USE_SERIAL.printf("%02X ", *src);
		src++;
	}
	USE_SERIAL.printf("\n");
}

void webSocketEvent(WStype_t type, uint8_t * payload, size_t length) {

	switch(type) {
		case WStype_DISCONNECTED:
			USE_SERIAL.printf("[WSc] Disconnected!\n");
			break;
		case WStype_CONNECTED:
			USE_SERIAL.printf("[WSc] Connected to url: %s\n", payload);

			// send message to server when Connected
			webSocket.sendTXT("Connected");
			break;
		case WStype_TEXT:
			USE_SERIAL.printf("[WSc] get text: %s\n", payload);

		// 	// send message to server
		// 	// webSocket.sendTXT("message here");
		// 	break;
		// case WStype_BIN:
		// 	USE_SERIAL.printf("[WSc] get binary length: %u\n", length);
		// 	hexdump(payload, length);

		// 	// send data to server
		// 	// webSocket.sendBIN(payload, length);
		// 	break;
		// case WStype_ERROR:			
		// case WStype_FRAGMENT_TEXT_START:
		// case WStype_FRAGMENT_BIN_START:
		// case WStype_FRAGMENT:
		// case WStype_FRAGMENT_FIN:
		// 	break;
	}

}




int status = WL_IDLE_STATUS;
//set default speed to 0 when no publish from ROS
float linear_x = 0;
float angular_z = 0;

void setup() {
  Serial.begin(9600);
  while ( status != WL_CONNECTED) {
    Serial.print("Attempting to connect to Network named: ");
    Serial.println(ssid);                   // print the network name (SSID);

    // Connect to WPA/WPA2 network:
    status = WiFi.begin(ssid, pass);
  }

  // print the SSID of the network you're attached to:
  Serial.print("SSID: ");
  Serial.println(WiFi.SSID());

  // print your WiFi shield's IP address:
  IPAddress ip = WiFi.localIP();
  Serial.print("IP Address: ");
  Serial.println(ip);


  // server address, port and URL
	webSocket.begin(serverAddress, port, "/");

  // event handler
	webSocket.onEvent(webSocketEvent);

	// try ever 5000 again if connection has failed
	webSocket.setReconnectInterval(5000);
  webSocket.enableHeartbeat(1000,1000,2);

}

void loop() {
  webSocket.loop();

}

 

